from time import sleep
import rospy
import math
import numpy as np
from geometry_msgs.msg import PoseStamped, Quaternion, Twist,Vector3, Pose
from nav_msgs.msg import  Odometry
from six.moves import xrange
from std_msgs.msg import Header
from threading import Thread
from tf.transformations import quaternion_from_euler, euler_from_quaternion

class Skid_Steer_Pose_Controller(object):
    def __init__(self,robotname="", max_vel_forward = 2, max_vel_rotation = 10, pos_error = 2,kp_l = 0.4, kp_w = 10) -> None:
        super().__init__()
        self.robot_name =  robotname
        self.pose_want  = Pose()
        self.real_pos_recv = rospy.Subscriber(self.robot_name + "/odom", Odometry, self.poseMessageReceived)
        self.control_pose_recv = rospy.Subscriber(self.robot_name+"/pose_want", Pose, self.poseControlReceived)
        self.cmd_vel_publisher= rospy.Publisher(self.robot_name+ "/cmd_vel", Twist,queue_size = 1000)

        self.kp_l  = kp_l
        self.kp_w  = kp_w
        self.max_vel_forward = max_vel_forward
        self.max_vel_rotation = max_vel_rotation
        self.pos_error = pos_error
        
        self.received_pose = False
        self.reached = False

        self.inforatecount = 0
        

    def publishVelocity(self):
        self.inforatecount = self.inforatecount % 10000 + 1

        dx = self.pose_want.position.x - self.x
        dy = self.pose_want.position.y - self.y
        
        if self.inforatecount % 40 ==0:
            rospy.loginfo("real_pos:(%0.2fm, dy:%0.2fm)"%(self.x,self.y))

            rospy.loginfo("want_pos:(%0.2fm, dy:%0.2fm)"%(self.pose_want.position.x,self.pose_want.position.y))
            rospy.loginfo("dx:%0.2fm, dy:%0.2fm"%(dx,dy))
            rospy.loginfo("yaw_real:%0.2f"%(self.rpy[2]))


        assert self.rpy[2] <= np.pi and self.rpy[2] >= -np.pi, "yaw angle should range from -pi to pi"

        yaw_wanted = np.arctan2(dy, dx)
        if self.inforatecount % 40 ==0:
            rospy.loginfo("yaw_wanted:%0.2f"%(yaw_wanted))
        
        dangle = np.mod(yaw_wanted - self.rpy[2],2*np.pi) 
        if dangle > np.pi:
            dangle  = dangle - 2* np.pi
        elif dangle< - np.pi:
            dangle  = dangle + 2* np.pi 
        
        delta_pos = np.linalg.norm([dx,dy])
        
        linvel = self.kp_l * delta_pos* np.abs(np.cos(dangle))
        angvel = self.kp_w * dangle
        if linvel >= self.max_vel_forward:
            linvel = self.max_vel_forward
        if angvel >= self.max_vel_rotation:
            angvel = self.max_vel_rotation

        if delta_pos <= self.pos_error:
            linvel = 0 
            angvel = 0
            self.reached = True
        else:
            self.reached = False
            
        if self.inforatecount % 40 ==0:
            rospy.loginfo("delta_pos:%0.2f"%delta_pos)  
            rospy.loginfo("linvel: %0.2f, angvel:%0.2f"%(linvel, angvel))

        msgv= Twist()
        msgv.linear.x = linvel
        msgv.angular.z = angvel
        self.cmd_vel_publisher.publish(msgv)

    def poseMessageReceived(self,msgp):
        self.x = msgp.pose.pose.position.x
        self.y = msgp.pose.pose.position.y
        rpy    = euler_from_quaternion([msgp.pose.pose.orientation.x,msgp.pose.pose.orientation.y,msgp.pose.pose.orientation.z,msgp.pose.pose.orientation.w])
        self.rpy = rpy
        if self.received_pose:
            self.publishVelocity()

    def poseControlReceived(self, msgp):
        self.received_pose = True
        self.pose_want.position  = msgp.position

if __name__ == "__main__":
    rospy.init_node("control_vel",anonymous= True)
    v = Skid_Steer_Pose_Controller()
    rospy.spin()